﻿using System;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Windows;
using System.Windows.Documents;
using System.Windows.Media;

using Coding4Fun.BoxingBots.Common;
using Coding4Fun.BoxingBots.Communication;
using Coding4Fun.BoxingBots.Robot;
using Coding4Fun.Kinect.KinectService.Common;
using Coding4Fun.Kinect.KinectService.Listeners;

using Microsoft.Kinect;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;

using Color = System.Windows.Media.Color;
using ColorImageFormat = Microsoft.Kinect.ColorImageFormat;

namespace Coding4Fun.BoxingBots.Boxer
{
	/// <summary>
	/// Interaction logic for MainWindow.xaml
	/// </summary>
	public partial class MainWindow : Window
	{
		//private Timer _timer;
		private BoxerBot _boxer;
		private SocketListener<RobotStatePacket, MovementDescriptorPacket> _dataListener;

		private const int ComLeftSidePortNumber = 13;
		private const int ComRightSidePortNumber = 14;

		readonly SolidColorBrush _red = new SolidColorBrush(Color.FromArgb(255, 220, 20, 60));
		readonly SolidColorBrush _green = new SolidColorBrush(Color.FromArgb(255, 0, 168, 119));

		private readonly RobotState _state = new RobotState();
		private InputMode _inputMode = InputMode.Network;
		
		private bool _lastInputToggle;
		private bool _tempOverride;
		private bool _movementOverride;
		private bool _lockSkeleton;

		private int _currentRobotHealth;

		private KinectSensor _kinect;
		private ColorListener _colorListener;

		private NetworkSettings _mySettings = Settings.NetworkSettings[Properties.Settings.Default.Player];

		private Timer _vibrationTimer;

		#region Dependency Properties
		public double LeftMotorFront
		{
			get { return (double)GetValue(FrontLeftProperty); }
			set { SetValue(FrontLeftProperty, value); }
		}

		// Using a DependencyProperty as the backing store for LeftMotorFront.  This enables animation, styling, binding, etc...
		public static readonly DependencyProperty FrontLeftProperty =
			DependencyProperty.Register("LeftMotorFront", typeof(double), typeof(MainWindow), new UIPropertyMetadata(0d));

		public double RightMotorFront
		{
			get { return (double)GetValue(FrontRightProperty); }
			set { SetValue(FrontRightProperty, value); }
		}

		// Using a DependencyProperty as the backing store for RightMotorFront.  This enables animation, styling, binding, etc...
		public static readonly DependencyProperty FrontRightProperty =
			DependencyProperty.Register("RightMotorFront", typeof(double), typeof(MainWindow), new UIPropertyMetadata(0d));

		public double LeftMotorBack
		{
			get { return (double)GetValue(RearLeftProperty); }
			set { SetValue(RearLeftProperty, value); }
		}

		// Using a DependencyProperty as the backing store for LeftMotorBack.  This enables animation, styling, binding, etc...
		public static readonly DependencyProperty RearLeftProperty =
			DependencyProperty.Register("LeftMotorBack", typeof(double), typeof(MainWindow), new UIPropertyMetadata(0d));

		public double RightMotorBack
		{
			get { return (double)GetValue(RearRightProperty); }
			set { SetValue(RearRightProperty, value); }
		}

		// Using a DependencyProperty as the backing store for RightMotorBack.  This enables animation, styling, binding, etc...
		public static readonly DependencyProperty RearRightProperty =
			DependencyProperty.Register("RightMotorBack", typeof(double), typeof(MainWindow), new UIPropertyMetadata(0d));

		#endregion

		public MainWindow()
		{
			InitializeComponent();
		}

		private void Window_Loaded(object sender, RoutedEventArgs e)
		{
			ConnectComPorts();

			InitializeDataListener();

			if(Properties.Settings.Default.UseKinectService)
			{
				InitializeKinect();
				InitializeKinectService();
			}

			var entries = Dns.GetHostEntry(Environment.MachineName).AddressList.Where(ip => ip.AddressFamily == AddressFamily.InterNetwork).ToArray();
			var ips = entries.Aggregate<IPAddress, string>(null, (current, entry) => current + (entry + Environment.NewLine));

			ServerIP.Text = ips;

			_vibrationTimer = new Timer(VibrationTick, null, TimeSpan.Zero, TimeSpan.FromSeconds(1));
			Thread t = new Thread(InputThread);
			t.IsBackground = true;
			t.Start();
		}

		private void VibrationTick(object state)
		{
			if(_movementOverride)
			{
				GamePad.SetVibration(PlayerIndex.One, 0.35f, 0.35f);
				Thread.Sleep(100);
			}
			GamePad.SetVibration(PlayerIndex.One, 0, 0);
		}

		private void InitializeKinect()
		{
			// TODO: handle kinect status stuffs
			if(KinectSensor.KinectSensors.Count == 0 || KinectSensor.KinectSensors[0].Status != KinectStatus.Connected)
				throw new ApplicationException("No Kinects found in Connected state!");

			_kinect = KinectSensor.KinectSensors[0];
			_kinect.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);

			_kinect.Start();
		}

		private void InitializeKinectService()
		{
			// services for others to connect to
			_colorListener = new ColorListener(_kinect, _mySettings.RobotColorPort, ImageFormat.Jpeg);
			_colorListener.Start();
		}

		private void InitializeDataListener()
		{
			_dataListener = new SocketListener<RobotStatePacket, MovementDescriptorPacket>();
			_dataListener.ConnectionCompleted += DataListenerConnectionCompleted;
			_dataListener.PacketReceived += DataListenerPacketReceived;

			_dataListener.Start(Settings.NetworkSettings[Properties.Settings.Default.Player].RobotDataPort);
		}

		private void DataListenerConnectionCompleted(object sender, ConnectionEventArgs e)
		{
			Debug.WriteLine("Connection to BoxingBot from Scoreboard: " + e.Connected);
			SetStatus(SocketStatus, e.Connected);
		}

		void DataListenerPacketReceived(object sender, PacketReceivedEventArgs<MovementDescriptorPacket> e)
		{
			Debug.WriteLine("MovementDescriptorPacket (robot): " + e.Packet);

			if(_inputMode == InputMode.Network)
				UpdateState(e.Packet);
		}

		private void ConnectComPorts()
		{
			bool connectedToRobot = false;

			try
			{
				_boxer = new BoxerBot(ComLeftSidePortNumber, ComRightSidePortNumber, Settings.IsRobotMotorsEnabled);
				connectedToRobot = true;
			}
			catch (IOException)
			{
				_boxer = new BoxerBot();
			}

			RotationVector.AllowedMovementArea =
				_boxer.AllowedRotationAngle = Settings.RobotDeadzone;
			
			MovementVector.AllowedMovementArea =
				_boxer.AllowedMovementArea = Settings.RobotDeadzone;

			_boxer.MaxThrottle =Settings.RobotSpeed;

			SetStatus(RobotStatus, connectedToRobot);
		}

		private void InputThread()
		{
			while(true)
			{
				GamePadState gamePadState = GamePad.GetState(PlayerIndex.One);
				SetStatus(ControllerStatus, gamePadState.IsConnected);

				if(gamePadState.IsConnected)
				{
					// toggle to encable Xbox mode, game state change
					bool inputToggle = (gamePadState.Triggers.Left > Settings.XboxTriggerThreshold);

					if(inputToggle && (inputToggle != _lastInputToggle))
					{
						_inputMode = _inputMode == InputMode.Xbox ? InputMode.Network : InputMode.Xbox;
						switch(_inputMode)
						{
							case InputMode.Xbox:
								_movementOverride = true;
								break;
							case InputMode.Network:
								_movementOverride = false;
								break;
							default:
								throw new ArgumentOutOfRangeException("_inputMode");
						}
					}

					_lastInputToggle = inputToggle;

					// hold to enable Xbox mode, no game state change
					if(gamePadState.Triggers.Right > Settings.XboxTriggerThreshold)
					{
						_tempOverride = true;
						_inputMode = InputMode.Xbox;
					}
					else if(_tempOverride)
					{
						_tempOverride = false;
						_inputMode = InputMode.Network;
					}
					
					if(_inputMode == InputMode.Xbox)
						UpdateState(gamePadState);

					_lockSkeleton = gamePadState.Buttons.Y == ButtonState.Pressed;
				}

				SensorData sensorData = UpdateRobot(_state);

				RobotStatePacket rsp = new RobotStatePacket
				{
					SensorData = new CalculatedSensorData
					{
						TopLeft = sensorData.TopLeft,
						TopRight = sensorData.TopRight,
						BottomLeft = sensorData.BottomLeft,
						BottomRight = sensorData.BottomRight,
						IsRightSwitchTriggered = sensorData.IsRightSwitchTriggered,
						IsLeftSwitchTriggered = sensorData.IsLeftSwitchTriggered,
					},
					PunchDetected = sensorData.IsSwitchTriggered,
					MovementOverride = _movementOverride,
					LockSkeleton = _lockSkeleton
				};

				_dataListener.Send(rsp);

				UpdateUI(_state, sensorData);

				// ~30fps
				Thread.Sleep(33);
			}
		}

		private void UpdateState(MovementDescriptorPacket movement)
		{
			_state.Strafe = movement.Strafe;
			_state.Move = movement.Move;
			_state.Rotate = movement.Rotate;

			_state.Speed = _boxer.CalculateSpeed(_state.Move, _state.Strafe, _state.Rotate);

			_state.HeadPopped = movement.PopHead;

			_state.PunchLeft = movement.PunchLeft;
			_state.PunchRight = movement.PunchRight;

			_currentRobotHealth = movement.Health;
		}

		private void UpdateState(GamePadState gamePadState)
		{
			_state.Strafe = gamePadState.ThumbSticks.Left.X;
			_state.Move = gamePadState.ThumbSticks.Left.Y;
			_state.Rotate = gamePadState.ThumbSticks.Right.X;

			_state.Speed = _boxer.CalculateSpeed(_state.Move, _state.Strafe, _state.Rotate);

			_state.HeadPopped = gamePadState.Buttons.A == ButtonState.Pressed;
				
			_state.PunchLeft = gamePadState.DPad.Left == ButtonState.Pressed;
			_state.PunchRight = gamePadState.DPad.Right == ButtonState.Pressed;
		}

		private SensorData UpdateRobot(RobotState state)
		{
			if (_boxer != null)
			{
				_boxer.Drive(state.Speed);

				_boxer.SetHealth(_currentRobotHealth);

				if(state.PunchLeft)
					_boxer.ExtendLeftArm();

				if(state.PunchRight)
					_boxer.ExtendRightArm();

				if(state.HeadPopped)
					_boxer.ExtendHead();

				return _boxer.SensorData;
			}

			return new SensorData();
		}

		private void UpdateUI(RobotState rs, SensorData sd)
		{
			Dispatcher.BeginInvoke(new Action(() =>
			{
				InputState.Text = _inputMode.ToString();

				SensorDataDisplay.FrontLeftSensor = sd.TopLeft;
				SensorDataDisplay.FrontRightSensor = sd.TopRight;
				SensorDataDisplay.RearLeftSensor = sd.BottomLeft;
				SensorDataDisplay.RearRightSensor = sd.BottomRight;

				SensorDataDisplay.AverageSensor = (sd.IsSwitchTriggered) ? 1 : 0;

				MovementVector.HorizontalValue = rs.Strafe;
				MovementVector.VerticalValue = rs.Move;
				RotationVector.HorizontalValue = rs.Rotate;

				LeftMotorFront = Math.Round(_state.Speed.LeftMotorFront, 2);
				RightMotorFront = Math.Round(_state.Speed.RightMotorFront, 2);
				LeftMotorBack = Math.Round(_state.Speed.LeftMotorBack, 2);
				RightMotorBack = Math.Round(_state.Speed.RightMotorBack, 2);

				if (_currentRobotHealth < BoxerHealth.Maximum)
					BoxerHealth.Value = _currentRobotHealth;

				RightPunchButton.Background = rs.PunchRight ? _green : null;
				LeftPunchButton.Background = rs.PunchLeft ? _green : null;
				ExtendHeadButton.Background = rs.HeadPopped ? _green : null;
			}));
		}

		private void SetStatus(Run sender, bool isConnected)
		{
			Dispatcher.BeginInvoke(new Action(() =>
			{
				sender.Text = (isConnected) ? "Connected" : "Not Connected";
				sender.Foreground = (isConnected) ? _green : _red;
			}));
		}

		private void MovementVectorDeadzoneChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
		{
			_boxer.AllowedMovementArea = e.NewValue;
		}
		
		private void RotationVectorDeadzoneChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
		{
			_boxer.AllowedRotationAngle = e.NewValue;
		}

		private void BoxerHealth_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
		{
			_currentRobotHealth = (int)e.NewValue;
		}

		private void Button_PreviewMouseLeftButtonDown(object sender, System.Windows.Input.MouseButtonEventArgs e)
		{
			_boxer.SensorData.IsLeftSwitchTriggered =
				_boxer.SensorData.IsRightSwitchTriggered = true;			
		}

		private void Button_PreviewMouseLeftButtonUp(object sender, System.Windows.Input.MouseButtonEventArgs e)
		{
			_boxer.SensorData.IsLeftSwitchTriggered =
				_boxer.SensorData.IsRightSwitchTriggered = false;
		}

	}
}
